#include "user_device.h"
#include "modbus.h"
#include "tim.h"
#include "usart.h"
#include "speed_cntr.h"
#include <setjmp.h>


jmp_buf main_loop_env;
UU16 system_tick = {0};

ushort SystemTickLow()
{
    ushort tick = timer_counter_read(TIMER6);
    return tick;
}

ushort SystemTickHigh()
{
    uchar hi, lo;

    do
    {
        hi = system_tick.UU8[MSB];
        lo = system_tick.UU8[LSB];
    }
    while(hi != system_tick.UU8[MSB]);

    return (ushort)hi << 8 | lo;
}

ulong SystemTick()
{
    ushort hi, lo;
    do
    {
        hi = SystemTickHigh();
        lo = SystemTickLow();
    }
    while(hi != SystemTickHigh());

    return (ulong)(hi << 16 | lo);
}

void Delay_Tick(ushort tick)
{
    ushort t = SystemTickLow();
    while((ushort)(SystemTickLow() - t) < tick)
        Update_WatchDog();
}

void Delay_Tick_Loop(ulong tick)
{
    ulong t = SystemTick();
    while(SystemTick() - t < tick)
        Main_Loop();
}

void Read_Address(void)
{
    server_address = ((IO_Read(IO_SWITCH3) << 2)|(IO_Read(IO_SWITCH2)  << 1)|
                      (IO_Read(IO_SWITCH1) << 0)) + 0x10;
}


void Device_Init(void)
{
    Read_Address();
    InitMachine();

    ro_bit.rack_ready_importl = IO_Read(IO_IMPORTL_READY);
    ro_bit.rack_ready_importr = IO_Read(IO_IMPORTR_READY);
    ro_bit.rack_ready_export = IO_Read(IO_EXPORT_READY);

    //debug
    rw_word.app_arg1.ignore_importl_ready = 0;
    rw_word.app_arg1.ignore_importr_ready = 0;
    rw_word.app_arg1.ignore_export_ready = 0;
    // gportin[IO_EXPORT_READY].inverse = 1;
    // gportin[IO_PREL_EXIST].inverse = 1;
    // gportin[IO_PRER_EXIST].inverse = 1;

    rw_word.action = MRESET;
    pending_action = 1;
    ro_word.rack_number = 0;
}

void Tst_Systick_Timer6(void)
{
   static ulong stamp;
   if(SystemTick() - stamp > US_TO_TICK(5000000))
   {
       LED_BLINK();
       stamp = SystemTick();
   }
}



